#include "usb_cam.h"

#include <sys/mman.h>
#include <assert.h>

#define CLEAR(x) memset(&(x), 0, sizeof(x))

using namespace usb_cam::utils;
namespace usb_cam
{

// #define WIDTH 1920
// #define HEIGHT 1200
// #define FRAMERATE 5

UsbCam::UsbCam(std::string name, size_t w, size_t h, size_t f): m_device_name(name), 
    m_fd(-1),
    m_number_of_buffers(4),
    m_buffers(new usb_cam::utils::buffer[m_number_of_buffers]),
    m_is_capturing(false),
    m_image(w, h, f),
    m_epoch_time_shift_us(usb_cam::utils::get_epoch_time_shift_us())
{
    
}
UsbCam::~UsbCam()
{
    shutdown();
    std::cout << "UsbCam Distroy"<< std::endl;
}

void UsbCam::configure()
{
    //TODO: zc param
    // m_image.size_in_bytes = WIDTH * HEIGHT * 2;
    // m_image.width = WIDTH;
    // m_image.height = HEIGHT;
    open_device();
    init_device();
}

void UsbCam::start()
{
    start_capturing();
}

void UsbCam::shutdown()
{
    stop_capturing();
    uninit_device();
    close_device();
}

// char* UsbCam::get_image()
// {
    
// }

void UsbCam::get_image(char * destination)
{
    // Set the destination pointer to be filled
    m_image.data = destination;
    // grab the image
    grab_image();
}

int UsbCam::init_device()
{
    //1、获取设备的信息
    struct v4l2_capability cap;
    if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_QUERYCAP), &cap)) {
        if (EINVAL == errno) {
            std::cout << "Device is not a V4L2 device" << std::endl;
        } else {
            std::cout << "Unable to query device capabilities"<< std::endl;
        }
        return -1;

    }

    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        std::cout << "Device is not a video capture device"<< std::endl;
    }

    // 第一步：将曝光模式设置为手动 (Manual Mode)
    if(m_set_exposure_flag){
        struct v4l2_control control_mode;
        control_mode.id = V4L2_CID_EXPOSURE_AUTO; // 控制曝光模式的ID
        control_mode.value = m_exposure_auto; // 手动模式，通常值为1。V4L2_EXPOSURE_APERTURE_PRIORITY 为光圈优先，通常值为3。
        if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_S_CTRL), &control_mode)) {
            std::cout << "设置曝光模式失败"<< std::endl;
            return -1;
        }

        if(m_exposure_auto == V4L2_EXPOSURE_MANUAL){
            struct v4l2_control control_absolute;
            control_absolute.id = V4L2_CID_EXPOSURE_ABSOLUTE; // 控制曝光绝对值的ID
            control_absolute.value = m_exposure_absolute; // 设置你想要的曝光值，这个值需要在摄像头支持的范围内

            if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_S_CTRL), &control_absolute)) {
                std::cout << "设置曝光绝对值失败"<< std::endl;
                return -1;
            }
        }
    }

    //2、设置参数 分辨率  图像格式
    struct v4l2_format fmt = {0};
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix_mp.width = m_image.width;
    fmt.fmt.pix_mp.height = m_image.height;
    fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_MJPEG;
    if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_S_FMT), &fmt)) {
        std::cout << "Set param failed"<< std::endl;
        return -1;
    }

    //3、设置帧率
    struct v4l2_streamparm stream_params;
    memset(&stream_params, 0, sizeof(stream_params));
    stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_G_PARM), &stream_params) < 0) {
        std::cout << "VIDIOC_G_PARM failed"<< std::endl;
        return -1;
    }

    if (!stream_params.parm.capture.capability && V4L2_CAP_TIMEPERFRAME) {
        std::cout << "V4L2_CAP_TIMEPERFRAME not supported"<< std::endl;
        return -1;
    }

    // TODO(lucasw) need to get list of valid numerator/denominator pairs
    // and match closest to what user put in.
    stream_params.parm.capture.timeperframe.numerator = 1;
    stream_params.parm.capture.timeperframe.denominator =m_image.framerate;
    if (usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_S_PARM), &stream_params) < 0) {
        std::cout << "Couldn't set camera framerate"<< std::endl;
    }

    //4、init mmap
    int ret = init_mmap();
    std::cerr << "init_device..." << std::endl;
    return ret;
}

int UsbCam::open_device()
{
    struct stat st;

    if (-1 == stat(m_device_name.c_str(), &st)) {
        std::cout << "设备符号不存在" << std::endl;
        return -1;
    }
    //判断是否为字符设备
    if (!S_ISCHR(st.st_mode)) {
        std::cout << "不是字符设备" << std::endl;
        return -1;
    }
    //读写 非阻塞
    m_fd = open(m_device_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);

    std::cout << "m_fd" << m_fd << std::endl;

    if (-1 == m_fd) {
        std::cout << "打开设备失败" << std::endl;
        return -1;
    }
    std::cout << "打开设备完成" << std::endl;
    return 0;
}

int UsbCam::grab_image()
{
    fd_set fds;
    struct timeval tv;
    int r;

    FD_ZERO(&fds);
    FD_SET(m_fd, &fds);
    /* Timeout. */
    tv.tv_sec = 2;
    tv.tv_usec = 0;
    r = select(m_fd + 1, &fds, NULL, NULL, &tv);

    if (-1 == r) {
        if (EINTR == errno) {
            // interruped (e.g. maybe Ctrl + c) so don't throw anything
            return -1;
        }

        std::cerr << "Something went wrong, exiting..." << errno << std::endl;
        return -1;
    }

    if (0 == r) {
        std::cerr << "Select timeout, exiting..." << std::endl;
        return -1;
    }
    read_frame();
    return 0;
}

void UsbCam::uninit_device()
{
    m_buffers.reset();
}

int UsbCam::close_device()
{
    if(-1 == m_fd) return 0;

    if(-1 == close(m_fd)){
        return -1;
    }
    m_fd = -1;
    std::cout << "close_device" << std::endl;
    return 0;
}

int UsbCam::init_mmap()
{
    struct v4l2_requestbuffers req;

    CLEAR(req);

    req.count = m_number_of_buffers;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_REQBUFS), &req)) {
        if (EINVAL == errno) {
            std::cout << "Device does not support memory mapping" << std::endl;
        } else {
            std::cout << "Unable to initialize memory mapping" << std::endl;
        }
        return -1;
    }

    if (req.count < m_number_of_buffers) {
        std::cout << "Insufficient buffer memory on device" << std::endl;
        return -1;
    }

    if (!m_buffers) {
        std::cout << "Out of memory" << std::endl;
        return -1;
    }

    for (uint32_t current_buffer = 0; current_buffer < req.count; ++current_buffer) {
        struct v4l2_buffer buf;

        CLEAR(buf);

        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = current_buffer;

        if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_QUERYBUF), &buf)) {
            std::cout << "Unable to query status of buffer" << std::endl;
            return -1;
        }

        m_buffers[current_buffer].length = buf.length;
        m_buffers[current_buffer].start =
        reinterpret_cast<char *>(mmap(
            NULL /* start anywhere */, buf.length, PROT_READ | PROT_WRITE /* required */,
            MAP_SHARED /* recommended */, m_fd, buf.m.offset));

        if (MAP_FAILED == m_buffers[current_buffer].start) {
            std::cout << "Unable to allocate memory for image buffers" << std::endl;
            return -1;
        }
    }

    return 0;
}

void UsbCam::read_frame()
{
    struct v4l2_buffer buf;

    CLEAR(buf);
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;

    struct v4l2_format fmt = {0};
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix_mp.width = m_image.width;
    fmt.fmt.pix_mp.height = m_image.height;
    fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_MJPEG;

    // Get current v4l2 pixel format
    if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_G_FMT), &fmt)) {
        switch (errno) {
        case EAGAIN:
            return;
        default:
            std::cout << "Invalid v4l2 format" << std::endl;
        }
    }
    /// Dequeue buffer with the new image
    if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_DQBUF), &buf)) {
        switch (errno) {
        case EAGAIN:
            return;
        default:
            std::cout << "Unable to retrieve frame with mmap" << std::endl;
        }
    }

    // Get timestamp from V4L2 image buffer
    m_image.stamp = usb_cam::utils::calc_img_timestamp(buf.timestamp, m_epoch_time_shift_us);

    assert(buf.index < m_number_of_buffers);
    //TODO ???
    m_image.size_in_bytes = buf.bytesused;
    memcpy(m_image.data, m_buffers[buf.index].start, buf.bytesused);

    /// Requeue buffer so it can be reused
    if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_QBUF), &buf)) {
        std::cout <<  "Unable to exchange buffer with the driver" << std::endl;
    }
    return;
       
}


void UsbCam::stop_capturing()
{
    if (!m_is_capturing) {return;}

    m_is_capturing = false;
    enum v4l2_buf_type type;

    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (-1 == usb_cam::utils::xioctl(m_fd, VIDIOC_STREAMOFF, &type)) {
    // Set capturing variable to true again, since stream was not stopped successfully
        m_is_capturing = true;
        std::cout << "Unable to stop capturing stream" << std::endl;
    }
    std::cout << "stop capturing stream" << std::endl;
    return;
}

int UsbCam::start_capturing()
{
    if (m_is_capturing) {return 0;}

    unsigned int i;
    enum v4l2_buf_type type;

    // Queue the buffers
    for (i = 0; i < m_number_of_buffers; ++i) {
        struct v4l2_buffer buf;
        CLEAR(buf);

        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;

        if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_QBUF), &buf)) {
            std::cout << "Unable to queue image buffer" << std::endl;
            return -1;
        }
    }

    // Start the stream
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (-1 == usb_cam::utils::xioctl(m_fd, VIDIOC_STREAMON, &type)) {
        std::cout << "Unable to start stream" << std::endl;
            return -1;
    }
    m_is_capturing = true;
    std::cout << "start capturing stream" << std::endl;
    return 0;
}


} // namespace usb_cam